#pragma config(Sensor, in1,    lnLeft,         sensorLineFollower)
#pragma config(Sensor, in2,    lnMid,          sensorLineFollower)
#pragma config(Sensor, in3,    lnRight,        sensorLineFollower)
#pragma config(Sensor, dgtl1,  snsBall,        sensorTouch)
#pragma config(Sensor, dgtl2,  encLeft,        sensorQuadEncoder)
#pragma config(Sensor, dgtl4,  encRight,       sensorQuadEncoder)
#pragma config(Sensor, dgtl6,  snsFront,       sensorSONAR_inch)
#pragma config(Motor,  port2,           leftWheels,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           rightWheels,   tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           treads,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           blocker,       tmotorServoStandard, openLoop)

//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

bool hasBall = false, atHoop = false, atTurn = false, checkingBumpSensor = false;
int bias = 50;
int valTape = SensorValue(lnMid) + bias;
int valOther = (SensorValue(lnLeft) + SensorValue(lnRight))/2 + bias;
int valBlack = 2; //ENTER IN VALUE OF BLACK TAPE
int perfectLeft = 75, perfectRight = 78;
int revLeft = -50 , revRight = -50;

void fwd(int time, int speedLeft, int speedRight){
  motor[leftWheels] = speedLeft;
  motor[rightWheels] = speedRight;
  wait1Msec(time);
}

void left(){
  motor[leftWheels] = -50;
  motor[rightWheels] = 50;
  wait1Msec(2500);
}

void right(){
  motor[leftWheels] = 50;
  motor[rightWheels] = +50;
  wait1Msec(2500);
}

void reset(){
  motor[leftWheels] = 0;
  motor[rightWheels] = 0;
  motor[treads] = 0;
}

task checkBumpSensor(){
  checkingBumpSensor = true;
  int numBumps = 0;
  for(int i = 0; i < 25; i ++){
    if(SensorValue(snsBall) == 1)
      numBumps++;
    wait1Msec(20);
  }
  if (numBumps >= 5)
    hasBall = true;
  else
    hasBall = false;
  checkingBumpSensor = false;
  numBumps = 0;
}
task main(){
  //Init Servo Down = -, Up = +
  motor[blocker] = -115;
  wait1Msec(1000);

  //Collect First Ball
  while(!hasBall){
    //motor[treads] = 127;
    /*
    if(SensorValue(lnMid) <= valTape)
      fwd(100,-perfectLeft, -perfectRight);
    else if (SensorValue(lnLeft) <= valTape)
      fwd(100,-perfectLeft,perfectRight);
    else if (SensorValue(lnRight) <= valTape)
      fwd(100,perfectLeft,-perfectRight);
    else if (SensorValue(lnLeft) > valTape && SensorValue(lnMid) > valTape && SensorValue(lnRight) > valTape)
      fwd(100,revLeft, revRight);
    */
    if((SensorValue(lnRight))<2800)
      fwd(100, -perfectLeft, perfectRight);//perfectRight   turns right towards line
    else if ((SensorValue(lnLeft))<2800){
      fwd(500, -perfectLeft, - perfectRight);
      fwd(100, perfectLeft, -perfectRight);//perfectLeft   turns left towards line
    }
    else if((SensorValue(lnMid))<2800){
      fwd(100,-perfectLeft,-perfectRight);
    }
    if(SensorValue(snsBall) == 1 && !checkingBumpSensor){
      StartTask(checkBumpSensor);
  }
  reset();
  //Reverse To Get Back On Line
  do{
    fwd(100, -revLeft, -revRight);
  }while(SensorValue(lnMid) > 2900);
  right();

  //Continue Onwards
  while(!atTurn){
    /*
    if(SensorValue(lnMid) <= valBlack || SensorValue(lnLeft) < valBlack || SensorValue(lnRight) < valBlack)
      atTurn = true;
    else if(SensorValue(lnMid) <= valTape)
      fwd(1000,perfectLeft, perfectRight);
    else if (SensorValue(lnLeft) < valOther)
      fwd(100,0,perfectRight);
    else if (SensorValue(lnRight) < valOther)
      fwd(100,perfectLeft,0);
    else if (SensorValue(lnLeft) > valTape && SensorValue(lnMid) > valTape && SensorValue(lnRight) > valTape)
      fwd(100,revLeft, revRight);
      */
    if((SensorValue(lnRight))<2800)
      fwd(100, perfectLeft, -perfectRight);//turns right towards line
    else if ((SensorValue(lnLeft))<2800)
      fwd(100, -perfectLeft, perfectRight);//turns left towards line
    else if((SensorValue(lnMid))<2800)
      fwd(100,perfectLeft, perfectRight);
  }
  left();
  fwd(1000,perfectLeft,perfectRight);
  right();

  //SONAR
  while(!atHoop)
  {
    if(SensorValue(snsFront)<=2)
    {
      motor[leftWheels]=0;
      motor[rightWheels]=0;
      atHoop=true;
    }
    else
    {
      if(SensorValue(lnMid) <= valTape)
        fwd(1000,perfectLeft, perfectRight);
      else if (SensorValue(lnLeft) < valOther)
        fwd(100,0,perfectRight);
      else if (SensorValue(lnRight) < valOther)
        fwd(100,perfectLeft,0);
      else if (SensorValue(lnLeft) > valTape && SensorValue(lnMid) > valTape && SensorValue(lnRight) > valTape)
        fwd(100,revLeft, revRight);
    }
  }
  //Drop Ball
  motor[blocker] = +127;
  motor[treads] = 127;
  wait1Msec(2000);
  reset();

}
